#include "skeletonExporter.h"

SkeletonExporter::SkeletonExporter() {
}

SkeletonExporter::~SkeletonExporter() {
}

void SkeletonExporter::write(Skeleton *s, string filename) {
   FileType id = noneFile;
   if (filename.substr(filename.size() - 4, 4) == ".asf") {
      id = asfFile;
   } else if (filename.substr(filename.size() - 4, 4) == ".bvh") {
      id = bvhFile;
   }
   if (id == noneFile) {
      cout << "File format not recognizeg!" << endl;
   } else {
      write(s, filename, id);
   }
}

void SkeletonExporter::write(Skeleton *s, string filename, FileType id) {
   switch (id) {
      case asfFile:
         return writeASF(s, filename);
         break;
      case bvhFile:
         return writeBVH(s, filename);
         break;
      default:
         break;
   }
}

void SkeletonExporter::writeASF(Skeleton *s, string filename) {
   Point3D rp = s->getRootPosition();
   string tr = s->getTrOrder();
   int id = 1;
   
   ofstream file(filename.c_str());
   file << "# ASF file generated by glutMOCAPviewer2.0" << endl;
   file << "# -----------------------------------------------" << endl;
   file << ":version 2.0" << endl;
   file << ":name glutMOCAPviewer2skel" << endl;
   file << ":units" << endl;
   file << "  mass 1.0" << endl;
   file << "  length 0.45" << endl;
   file << "  angle deg" << endl;
   file << ":documentation" << endl;
   file << "   .ast/.asf automatically generated by glutMOCAPviewer2.0" << endl;
   
   file << ":root" << endl;
   file << "   order T" << tr[0] << " T" << tr[1] << " T" << tr[2] << " R"
         << tr[3] << " R" << tr[4] << " R" << tr[5] << endl;
   file << "   axis XYZ" << endl;   
   file << "   position " << s->getRootPosition() << endl;  
   file << "   orientation 0 0 0" << endl;
   
   file << ":bonedata" << endl;
   map<string, Bone> b = s->getBones();
   for (map<string, Bone>::iterator i = b.begin(); i != b.end(); i++) {
      Bone bo = i->second;
      Vector3D dir = bo.getDirection();
      Vector3D rotAxis = bo.getRotationAxis();
      string dof = bo.getAxisOrder();
      file << "  begin" << endl;
      file << "     id " << id << endl; 
      file << "     name " << i->first << endl;
      file << "     direction " << dir[0] << " " << dir[1] << " " << dir[2] << endl;
      file << "     length " << bo.getLength() << endl;
      file << "     axis " << rotAxis[0] << " " << rotAxis[1] << " " << rotAxis[2] << " " << bo.getRotAxisOrder() << endl;
      file << "     dof";
      for (int i = 0; i < dof.size(); i++){
         file << " r" << dof[i];
      }
      file << endl;
      file << "  end" << endl;
      id++;
   }
   
   file << ":hierarchy" << endl;   
   Hierarchy h = s->getHierarchy();
   file << "  begin" << endl;
   //file << "    root lhipjoint rhipjoint lowerback" << endl;
   for (Hierarchy::iterator i = h.begin(); i != h.end(); i++) {
      file << "    " << i->first << " ";
      vector<string> v = i->second;
      for(vector<string>::iterator it = v.begin(); it != v.end(); it++){
         file << *it << " ";
      }
      file << endl;
   }
    file << "  end" << endl;
    file.close();
    cout << "Skeleton exported succefully!" << endl;
}

void SkeletonExporter::writeBVH(Skeleton *s, string filename) {

}
